#pragma once
#ifndef UKF_HPP_
#define UKF_HPP_

#include <iostream>
#include <stdlib.h>
#include <string>
#include <Eigen/Geometry>
#include "Eigen/Dense"
#include <unordered_map>
#include <vector>
#include <algorithm>
#include <random>


// 当a和b的量测时间小于DELTA_TIME_FOR_UKF_PROCRSS时，则认为a与b的量测是在同一时间进行的
#define DELTA_TIME_FOR_UKF_PROCRSS uint64_t(10)
#define Initial_P_sigma 0.01

extern const Eigen::MatrixXd default_P;

// UKF_Output_
// 输出 或 上次输出结果（作为初始状态）
struct Filtered_Target_State
{
	// 基准时间
	uint64_t FilterTime;

	// // 若计算不成功则不更新该值
	// bool StateIsComputed = false;

	Eigen::VectorXd State;

	Eigen::MatrixXd P;
};

// 输入 UKF_Input_
// 由于创建输入时会根据时序增加元素，一般无需额外排序
struct MeasureMents_In_Period
{
	// 该段时间的均值（由于时间差过小，将这段时间的量测集合视作同一时间的量测）
	uint64_t meanTime;

	Eigen::MatrixXd R_merge;

	// 与R对应的观测向量 [u1,v1,...un,vn]
	Eigen::VectorXd MeasurementsVector;

	// 与观测向量对应的观测矩阵集
	std::vector<Eigen::MatrixXd> MeasurementsMatrix;

	std::vector<double> Airplane_Z;

	bool operator < (const MeasureMents_In_Period &y) const
    {
        return meanTime < y.meanTime; //设定按照时间由小到大排列
    }
};

/**
 *  IMM_UKF 所使用的输出
 */



/**
 * IMM_UKF 所使用的输入
 */


/**
 * @brief 单个UKF
 * 			 UKF发散或目标丢失时间超过限度后，记得将is_initalized_置false
 * @param 初次输入:
 * 			S2计算所得飞机初始状态 X0 （Filtered_Target_State内）
 * 			S2量测的时间均值  （Filtered_Target_State内）
 * 			每个模型的初始协方差P0 （内置默认值）
 * 			融合的测量噪声阵 Eigen::MatrixXd R_merge_
 * 			本模型的Qj（内置默认值）
 * 			本模型的类型 1 2 3 (构造函数中产生)
 * @param 每步输入:(融合成多个MeasureMents_In_Period，根据时间升序排列)
 * 			该时间段的量测时间均值
 * 			该时间段所有飞机的量测数据集合 std::map<uint64_t,<std::vector<Measurement_Pair>> Measurements
 * 			观测矩阵集
 * 			该时间段融合所得的R矩阵
 * 			混合估计所计算的X和P
 * @result 输出Filtered_Target_State
 */
class UKF{
public:

	// 构造函数所使用的参数不可随意更改，最好在初始化时更改
	UKF(std::string smodel, Eigen::MatrixXd Q, int state_n = 10, float alpha = 0.001f, float beta = 2.0f);

	~UKF(){
	};

	/**
	 * @brief UKF发散或目标丢失时间超过限度后，
	 * 		丢弃所有已有数据，初始化状态重置
	 */
	bool TargetLost();

	// 检测该模型是否初始化成功
	bool is_initalized_ = false;
	// void Initialization(Eigen::VectorXd& X, Eigen::MatrixXd& P, float time);


	// 设置P0，X0
	//	void Initialization(Filtered_Target_State Initial_State);

	void Initialization(Filtered_Target_State Initial_State, Eigen::MatrixXd Q, float alpha = 0.001f, float beta = 2.0f);

	//// 多步量测处理
	// 第一步输入IMM重初始化的初始X P
	// 最后一步生成并输出结果到IMM
	// 中间调用UKF_Update
	// 输出滤波后的X，P和时间，S，测量误差用于模型融合
	bool Process(Filtered_Target_State &UKF_Output, double &likelihood, 
				 std::vector<MeasureMents_In_Period> UKF_Inputs,
				 Eigen::VectorXd X, Eigen::MatrixXd P);		 

	bool GetFilteredState(Filtered_Target_State &UKF_Output);

	bool Isinitalized();

	// void MakeSigmaPoints();



	// void Prediction(float ntime);

	// void PredictionZ(Eigen::VectorXd& X,Eigen::MatrixXd& P, float ntime);

	// void Update( std::vector<Eigen::VectorXd>& Z, const Eigen::VectorXd& beta, const float& last_beta);

	// void Update( Eigen::VectorXd& Z);

	// void Process(Eigen::VectorXd& X, Eigen::VectorXd& Z, float time);
	// void Process(Eigen::VectorXd& X, std::vector<Eigen::VectorXd>& Z, const Eigen::VectorXd& beta,const float& last_beta, Eigen::MatrixXd& P, float time);

	

	

	// Eigen::VectorXd Get_Zminus();

	// Eigen::MatrixXd Get_S();

	// Eigen::VectorXd Get_PredictionZ();

	// // 收到上次融合时间之前的测量数据时，此处deltatime为负数，等价于矩阵求逆
    // Eigen::MatrixXd Create_Prediction_Matrix(float detatime,float w);
	
	// void Init_R_and_P();

    // // 创建对应的观察矩阵
    // void UKF::Init_H_MAtrix(Eigen::Matrix4d Tc_G,
    //                    Eigen::Matrix4d Tg_B,
    //                    Eigen::Matrix4d Tb_N,
    //                    Eigen::Matrix4d Tn_W,
    //                    Eigen::MatrixXd camer_Mat);




	//////// 外部可用参数设置接口
	// 算法运行过程中需要缓存设定
	//// 参数
	bool Set_Q_(Eigen::MatrixXd Q);

	//// 以下两个参数修改时需要重新计算两种权值
	bool Set_alpha_(float alpha);

	bool Set_beta_(float beta);

	//// 每步被IMM_UKF调用
	// bool Set_P_(Eigen::MatrixXd P);//初值/上个状态的协方差，需要避免更改

	// bool Set_X_(Eigen::VectorXd X);

	Eigen::VectorXd Get_state();

	Eigen::MatrixXd Get_covariance();


private:

	/**
	 * @brief 根据UKF单步输入执行更新
	 * 		单步计算不做融合，只在迭代到最后一步融合三个模型
	 */
	bool UKF_Update(MeasureMents_In_Period UKF_Input);
	bool UKF_Update(Filtered_Target_State &output, double &likelihood_output, MeasureMents_In_Period UKF_Input);


	//// 下列函数在UKF_Update中调用
	// 生成sigma点
	Eigen::MatrixXd MakeSigmaPoints();

	// 计算时间间隔，更新运动模型
	// 其中，若当前时间小于上次时间，使用逆运动模型；若当前测量时间在上次输出结果时间之后，则使用原运动模型
	// 若时间间隔为0，抛出异常
	bool UpdateKinematicMatrix_(Eigen::MatrixXd &KinematicMatrix, uint64_t MeasureTime , uint64_t pretime, int model);
	void GenerateCA(Eigen::MatrixXd &KinematicMatrix, float deltaTime);
	void GenerateCV(Eigen::MatrixXd &KinematicMatrix, float deltaTime);
	void GenerateCT(Eigen::MatrixXd &KinematicMatrix, float deltaTime, float w);

	// 计算sigma点的预测，均值X_hat P_hat
	bool Prediction(Eigen::VectorXd &X_hat,Eigen::MatrixXd &P_hat, Eigen::MatrixXd &sigma_points_pre,
					Eigen::MatrixXd sigma_points, Eigen::MatrixXd KinematicMatrix, 
					int n_x, Eigen::MatrixXd Q, Eigen::VectorXd mean_weights, Eigen::VectorXd cov_Weights);

	// 由多个sigma点和实际测量计算的观测预测 Zk, sigma预测zk1temp
	bool PredictionZ_hat(Eigen::VectorXd &mean_Measurements_hat, Eigen::MatrixXd &Sigma_Measurements_hat,
					 Eigen::VectorXd mean_weights, Eigen::MatrixXd sigma_points_pre,
					 std::vector<Eigen::MatrixXd> MeasurementsMatrix, std::vector<double> Z);
	// // 多个sigma_pre 计算多个观测向量
	// bool GenerateMergeMeasurements(Eigen::MatrixXd &Sigma_Measurements_hat,
	// 								Eigen::MatrixXd sigma_points_pre,std::vector<Eigen::MatrixXd> MeasurementsMatrix);
	// 由单个sigma_pre 计算该sigma_pre所对应的合并观测向量
	bool GenerateMergeMeasurement(Eigen::VectorXd &Sigma_Measurement_hat, Eigen::VectorXd sigma_point_pre, std::vector<Eigen::MatrixXd> MeasurementsMatrix);
	// 根据状态值计算相应的像素位置值
    Eigen::Vector2d GeneratePictureLocation(float x,float y,float z);

	// 量测更新：根据合并的R，所有sigma的观测预测向量，和Wc，预测协方差S和T，
	// 用S T Zk 输入的实际观测MeasurementsVector，计算该模型的X1 P1, （卡尔曼增益）
	//  输入Zk， sigma观测预测，实际观测， R， Wc， 
	bool UpdateState(Eigen::VectorXd &State,Eigen::MatrixXd &P, Eigen::MatrixXd &S, Eigen::VectorXd &Zminus, double &likelihood,
					Eigen::MatrixXd Sigma_Measurements_hat, Eigen::VectorXd mean_Measurements_hat,
					Eigen::VectorXd X_hat, Eigen::MatrixXd P_hat,
					Eigen::MatrixXd sigma_points_pre,
					Eigen::VectorXd MeasurementsVector,
					int n_x, 
					Eigen::MatrixXd R_merge, Eigen::VectorXd cov_Weights);


	// 只有最后一次更新需要更新和输出S，Zminus，和 likelihood
	bool UpdateState(Eigen::VectorXd &State,Eigen::MatrixXd &P,Eigen::MatrixXd &S, Eigen::VectorXd &Zminus, 
					Eigen::MatrixXd Sigma_Measurements_hat, Eigen::VectorXd mean_Measurements_hat,
					Eigen::VectorXd X_hat, Eigen::MatrixXd P_hat,
					Eigen::MatrixXd sigma_points_pre,
					Eigen::VectorXd MeasurementsVector,
					int n_x, 
					Eigen::MatrixXd R_merge, Eigen::VectorXd cov_Weights);



	double Updatelikelihood(Eigen::VectorXd &Zminus, Eigen::MatrixXd &S);

	void GenerateSigmaWeights_(Eigen::VectorXd &mean_weights, Eigen::VectorXd &cov_weights, double &lambda, double n_x, double alpha, double beta);

	Eigen::MatrixXd K_;

	float gamma_P = 0.05;
	//float gamma_R = 0.05;//观测噪声设定

    // 观测向量的维度
	int n_z_;
    // 状态向量的维度 L
	int n_x_;
    // lamda参数，用于生成sigma points
	double lambda_;

	float alpha_ = 0.001;

	float beta_ = 2;

	double x_diff_gamma_ = 1;

	double z_diff_gamma_ = 1;

	double change_gamma_ = 0.4;


    // 上帧处理的时间点
	// float pretime;

	uint64_t pretime_;

	bool weight_make_ = false;

	std::unordered_map<std::string, int> model_hash_ = {{"CV",1},{"CA",2},{"CT",3}};
    // 模型类型 对象构造时固定一个值，后续不允许更改
	int model_ = 1;
	
    // // 状态向量  时刻变化
	// Eigen::VectorXd x_;
    // // 状态协方差矩阵  时刻变化
	// Eigen::MatrixXd P_;

    // 预测噪声协方差矩阵
	Eigen::MatrixXd Q_;
    // // 观察噪声协方差矩阵: 时刻变化
	//  Eigen::MatrixXd R_; 

    // 预测协方差权重 Wc
	Eigen::VectorXd cov_weights_;
    // 预测军均值权重 Wm
	Eigen::VectorXd mean_weights_;

    // sigma点 时刻变化
  	Eigen::MatrixXd sigma_points_;
    // sigma点预测值 时刻变化
  	Eigen::MatrixXd sigma_points_pre_;
    // // sigma点的观测值 时刻变化
	// Eigen::MatrixXd Z_sigma_;


	//集合计算后输入到IMM中使用
	Eigen::MatrixXd S_;
 	Eigen::VectorXd Zminus_;
	double likelihood_;
    // // 预测观测量
	// Eigen::VectorXd z_pre_;

	Eigen::MatrixXd KinematicMatrix_;

    // Eigen::MatrixXd CA;
    // Eigen::MatrixXd CV;
    // Eigen::MatrixXd CT;
    Eigen::MatrixXd H;

	// Eigen::MatrixXd Qcv;
	// Eigen::MatrixXd Qct;
	// Eigen::MatrixXd Qca;

	// 存储观测位置矩阵
	Eigen::MatrixXd XYZ_N;


	// UKF本次输入
	MeasureMents_In_Period UKF_Input_;
	// UKF的上次迭代输出
	Filtered_Target_State UKF_Output_;
};
#endif



